/*
 * Determine what speed commands do to the robot. 
 * Accept input speed and sets the speed to that number.
 * 
 * Determines how fast robot goes for input speed.
 */

#include <libplayerc++/playerc++.h>
#include <iostream>

#include "args.h"

#define RAYS 32

int
main(int argc, char **argv)
{
  parse_args(argc,argv);

  // we throw exceptions on creation if we fail
  try
  {
    using namespace PlayerCc;

    PlayerClient robot(gHostname, gPort);
    Position2dProxy pp(&robot, gIndex);
    LaserProxy lp(&robot, gIndex);

    //std::cout << robot << std::endl;

    pp.SetMotorEnable (true);


    // go into read-think-act loop
    for(;;)
    {
      double newspeed = 0;
      double newturnrate = .325;

      // this blocks until new data comes; 10Hz by default
      robot.Read();
      
      double speed;
      std::cout << "Enter a speed: ";
      std::cin >> speed;
      newspeed=speed;

      double turnrate;
      std::cout << "Enter a turnrate: ";
      std::cin >> turnrate;
      newturnrate=turnrate;

      std::cout << "speed: " << newspeed << std::endl
		<< "turn: " << newturnrate << "\n\n";

      // write commands to robot
      pp.SetSpeed(newspeed, newturnrate);
    }
  }

  catch (PlayerCc::PlayerError e)
  {
    std::cerr << e << std::endl;
    return -1;
  }
}
